import pyzed.sl as sl
import time
import redis
import math
import json


def asmaa(zed):
    transform = sl.Transform()
    tracking_params = sl.PositionalTrackingParameters(transform)
    zed.enable_positional_tracking(tracking_params)
    runtime_parameters = sl.RuntimeParameters()

    pose = sl.Pose()
    py_translation = sl.Translation()

    try:
        while True:
            if r.get('vio_switch').decode("utf-8") == 'off':
                raise KeyboardInterrupt

            if zed.grab(runtime_parameters) == sl.ERROR_CODE.SUCCESS:
                tracking_state = zed.get_position(pose)

                if tracking_state == sl.POSITIONAL_TRACKING_STATE.OK:
                    translation = pose.get_translation(py_translation)

                    tx = round(translation.get()[0], 2)
                    ty = round(translation.get()[1], 2)
                    tz = round(translation.get()[2], 2)

                    ratio = 1.229
                    print(tx, ty, tz)

                    r.lpush('vio_queue', json.dumps([tx*ratio, ty*ratio, tz*ratio, time.time()]))
                    r.ltrim('vio_queue', 0, 1000)
                    # print(math.sqrt((tx*r)**2 + (ty*r)**2 + (tz*r)**2))
    except KeyboardInterrupt:
        zed.disable_positional_tracking()


if __name__ == "__main__":
    r = redis.Redis(db=0)

    init_params = sl.InitParameters(camera_resolution=sl.RESOLUTION.HD720,
                                    depth_mode=sl.DEPTH_MODE.PERFORMANCE,
                                    coordinate_units=sl.UNIT.METER,
                                    coordinate_system=sl.COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP,
                                    sdk_verbose=True)

    zed = sl.Camera()

    err = zed.open(init_params)
    if err != sl.ERROR_CODE.SUCCESS:
        exit(1)

    while True:
        if r.get('vio_switch').decode("utf-8") == 'on':
            asmaa(zed)



